#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui_c.h>
#include "/home/usuario_robot/Pablo/sandbox/pruebaOp/include/pruebaOp/Handtracking.hpp"
#include <iostream>
#include <stdio.h>

namespace enc = sensor_msgs::image_encodings;

using namespace cv;
using namespace std;

const char WINDOW[] = "Image window";
const char WINDOW2[] = "Image window2";
const char entrada1[] = "rgb/image_raw";
const char entrada2[] = "depth/image_raw";
const char window_name[] = "Capture - Face detection";
Handtracking H;


class ImageConverter
{
  ros::NodeHandle nh_;
  image_transport::ImageTransport it_;
  image_transport::Subscriber image_sub_;
  image_transport::Publisher image_pub_;
  int j;

  
public:
  ImageConverter(int i)
    : it_(nh_)
  {
    
    j=i;
    //image_pub_ = it_.advertise("out", 1);
if (i)
    image_sub_ = it_.subscribe(entrada1, 1, &ImageConverter::imageCb, this);
else
    image_sub_ = it_.subscribe(entrada2, 1, &ImageConverter::imageCb, this);
if (i){}
    //namedWindow(WINDOW);
else{}
    //namedWindow(WINDOW2);
  }

  ~ImageConverter()
  {
if (j)
    destroyWindow(WINDOW);
else{}
    //destroyWindow(WINDOW2);
    
  }
 
  Mat backproject(Mat frame){

  Mat src;
  Mat het[3];
  src=frame; 
  split(src,het);
  Mat R,G,B;
  R.create(het[0].size(),het[0].depth());
  for(int i=0;i<het[0].rows-1;i++){
   for(int j=0;j<het[0].cols-1;j++){
   if((het[0].at<int>(i,j) > 95) & (het[1].at<int>(i,j) > 40) & (het[2].at<int>(i,j) > 20)
   & (abs(het[0].at<int>(i,j)-het[1].at<int>(i,j)) >15) & (het[0].at<int>(i,j)>het[1].at<int>(i,j))
   & (het[1].at<int>(i,j)>het[2].at<int>(i,j)) ){
   uchar* p = R.ptr(i); // returns a row pointer
   p[j] = 0;
   }
   else{
   uchar* p = R.ptr(i); // returns a row pointer
   p[j] = 255;
   }
   }
  }

  imshow( "Skin", R );
  return frame;

  }



  void imageCb(const sensor_msgs::ImageConstPtr& msg)
  {

   
    cv_bridge::CvImagePtr cv_ptr;
    try
    {
     if(j)      cv_ptr = cv_bridge::toCvCopy(msg, enc::BGR8);
     else       cv_ptr = cv_bridge::toCvCopy(msg, enc::TYPE_16UC1);
    }
    catch (cv_bridge::Exception& e)
    {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    //if(j)display = backproject(cv_ptr->image);
    if(!j) {
    H.HandPixel(cv_ptr->image);
    }
   else {
    H.anadir_frame(cv_ptr->image);
    }

    if(j){
    //imshow(WINDOW,cv_ptr->image);
    }
    else{}
   // imshow(WINDOW2,display);
    waitKey(3);
    
  }

};


int main(int argc, char** argv)
{ 
  ros::init(argc, argv, "image_converter");
  ImageConverter ic1(1);
  ImageConverter ic2(0);
  ros::spin();
  return 0;
/*
    Mat src = imread("/home/usuario_robot/Pablo/frame0000.jpg");
    if (src.empty())
        return -1;

    Mat hsv;
    cvtColor(src, hsv, CV_BGR2HSV);

    Mat bw;
    inRange(hsv, Scalar(3,40,100), Scalar(40,150,251), bw);
    imshow("BW",bw);
    vector<vector<Point> > contours;
    findContours(bw.clone(), contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

    Mat dst = Mat::zeros(src.size(), src.type());
    drawContours(dst, contours, -1, Scalar::all(255), CV_FILLED);

    dst &= src;

    imshow("src", src);
    imshow("dst", dst);
    waitKey(0);

    return 0;*/
}
